#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>

using namespace std::chrono_literals;

/* ================= 普通变量 ================= */
const std::vector<float> xxz_3s = {2346.0f, -2346.0f, 2170.0f};
const std::vector<float> xxz_5s = {1500.0f, -1500.0f, 1500.0f};
const std::vector<float> xxz_6s = {3000.0f, -1000.0f, 2500.0f};

const int T_3s = 30; // 3 s → 30*100 ms
const int T_5s = 50; // 5 s → 50*100 ms
const int T_6s = 60; // 6 s → 60*100 ms

int count_num = 0;
int state = 0; // 0: 3s, 1:5s, 2:6s

/* ================= 节点 ================= */
class JointPublisher : public rclcpp::Node
{
public:
    JointPublisher()
        : Node("joint_publisher_node")
    {
        pub_ = create_publisher<std_msgs::msg::Float32MultiArray>("/float_array_topic", 10);
        pub2_ = create_publisher<std_msgs::msg::Int32>("/control_mode", 10);
        timer_ = create_wall_timer(100ms, std::bind(&JointPublisher::timer_callback, this));
    }

private:
    void timer_callback()
    {
        std_msgs::msg::Float32MultiArray msg;
        std_msgs::msg::Int32 msg2;
        switch (state)
        {
        case 0:
            msg.data = xxz_3s;
            msg2.data = 3; // 上下扫刷
            pub_->publish(msg);
            pub2_->publish(msg2);
            RCLCPP_INFO(this->get_logger(), "Published 3s message: [xright: %f, xleft: %f, z: %f]", msg.data[0], msg.data[1], msg.data[2]);
            count_num++;
            RCLCPP_INFO(this->get_logger(), "已进行：%.1f s", count_num * 0.1f);
            if (count_num >= T_3s)
            {
                count_num = 0;
                state = 1; // 切换到5s状态
            }
            break;
        case 1:
            msg.data = xxz_5s;
            msg2.data = 2; // 回指定位置
            pub_->publish(msg);
            RCLCPP_INFO(this->get_logger(), "Published 指定位置 message: [xright: %f, xleft: %f, z: %f]", msg.data[0], msg.data[1], msg.data[2]);
            count_num++;
            RCLCPP_INFO(this->get_logger(), "已进行：%.1f s", count_num * 0.1f);
            if (count_num >= T_5s)
            {
                count_num = 0;
                state = 2; // 切换到6s状态
            }
            break;
        case 2:
            msg.data = xxz_6s;
            pub_->publish(msg);
            RCLCPP_INFO(this->get_logger(), "Published 6s message: [xright: %f, xleft: %f, z: %f]", msg.data[0], msg.data[1], msg.data[2]);
            count_num++;
            RCLCPP_INFO(this->get_logger(), "已进行：%.1f s", count_num * 0.1f);
            if (count_num >= T_6s)
            {
                count_num = 0;
                state = 3; // 切换到3s状态
            }
            break;
        case 3:
            msg.data = xxz_5s;
            msg2.data = 2; // 回指定位置
            pub_->publish(msg);
            RCLCPP_INFO(this->get_logger(), "结束 回指定位置 ");
            break;
        }
    }

    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr pub_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub2_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<JointPublisher>());
    rclcpp::shutdown();
    return 0;
}